import sensor,image,lcd,time



#常用初始化
lcd.init()
sensor.reset()                      #复位摄像头
sensor.set_pixformat(sensor.RGB565) #设置像素格式 RGB565
sensor.set_framesize(sensor.QVGA)   #设置帧尺寸 QVGA (320x240)
#sensor.set_windowing((224, 224))
sensor.set_vflip(1)
sensor.skip_frames(time = 2000)     #跳过不稳定画面



#焦距及其参数 f=D*V/v(1) f=D*H/h(2)
Focal_length=0 #焦距
cammer_v=0   # 相机画面的水平方向高度（如帧尺寸为QVGA时，cammer_v=320，cammer_h=240）
cammer_h=0   # 相机画面的垂直方向高度
distance=0   # 镜头到拍摄景物间的距离
entity_V=0   # 画面中框选目标的水平方向高度
entity_H=0   # 画面中框选目标的垂直方向高度
total=0
cnt=0
#红色阈值
red_threshold = (87, 21, 27, 93, -5, 92)
#绿色阈值
green_threshold = (100, 0, -16, -56, -128, 127)
#蓝色
blue_thresfold = (100, 72, 127, 27, -128, 8) #(14, 87, -83, 127, -113, -14)
#寻找最大色块函数定义
def find_max(blobs):
    max_size=0
    for blob in blobs:
        if blob[2]*blob[3] > max_size:
            max_blob=blob
            max_size = blob[2]*blob[3]
    return max_blob

while True:
    img=sensor.snapshot()
    blobs=img.find_blobs([green_threshold],merge=True)#把拍摄的一张图片里满足的色块纳入集合中
    if blobs:
                max_blob = find_max(blobs)#调用函数，返回最大色块

    img.draw_rectangle((max_blob.x(),max_blob.y(),max_blob.w(),max_blob.h()),color=(255,0,0))#用红色框出最大色块
    img.draw_string(max_blob.x(),max_blob.y(), "(x,y)=")
    img.draw_string(max_blob.x()+40,max_blob.y(), str(max_blob.cx()))
    img.draw_string(max_blob.x()+60,max_blob.y(), str(max_blob.cy()))#在框图左上角显示色块的中心坐标

    '''
    #测焦距
    #distance：物体的宽或高=画面的宽或高时，镜头到拍摄景物间的距离
    cammer_h=240
    entity_H=max_blob.h()
    distance=6
    Focal_length=distance*(entity_H/cammer_h)
    print("Focal_length:",Focal_length)
    '''

    #测距离
    cammer_h=240
    Focal_length=5
    entity_H=max_blob.h()
    distance=Focal_length*(cammer_h/entity_H)
    print("distance:",distance)
    num=float(distance)
    total+=num
    cnt+=1
    print("average distance:",total/cnt)

    #原初公式（1）（2）
    '''
    bp1=max_blob.w()
    print("distance:",3.7*(320/bp1))

    bp2=max_blob.h()
    print("distance1:",5*(240/bp2))
    '''
    lcd.display(img)


